#include "RGB.h"
#include "drv_flash.h"
#include "robot_control.h"
#include "board.h"
#include "rtthread.h"

#define LOG_TAG              "robottask"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>

void BMI088_ERASE_Callback()
{
    stm32_flash_erase((uint32_t)0x080E1000, 21);
    aRGB_led_show(LED_Blue);
    LOG_D("erase flash success\n");
}

void robotcmdtask(void *parameter)
{
    while (1)
    {
        robot_control();  
        rt_thread_mdelay(2);
    }
}



int robotcmd_thread_entry_init(void)
{
    rt_err_t ret = RT_EOK;
    rt_thread_t thread = rt_thread_create("robotcmd", robotcmdtask, RT_NULL, 4096, 29, 10);
    /* 创建成功则启动线程 */
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        ret = RT_ERROR;
    }

    LOG_D("robotcmd thread init success!\n");
    
    robot_control_init();
    rt_pin_mode(GET_PIN(A,0),PIN_MODE_INPUT_PULLDOWN);
    rt_pin_attach_irq(GET_PIN(A, 0), PIN_IRQ_MODE_RISING, BMI088_ERASE_Callback, RT_NULL);
    rt_pin_irq_enable(GET_PIN(A,0),ENABLE);

    return ret;
}
INIT_APP_EXPORT(robotcmd_thread_entry_init);